void OSCacc(String theString) {
  String[] values = split(theString, ',');
  if (values.length > 2) {
    x_accelQ = int(values[0]);
    y_accelQ = int(values[1]);
    z_accelQ = int(values[2]);
  }
}


void OSCgyr(String theString) {
  String[] values = split(theString, ',');
  if (values.length > 2) {
    x_rateQ = int(values[0]);
    y_rateQ = int(values[1]);
    z_rateQ = int(values[2]);
  }
}

void OSCalt(String theString) {
  String[] values = split(theString, ',');
  if (values.length > 3) {
    altQ = int(values[0]);
    agl_PQ = int(values[1]);
    agl_SQ = int(values[2]);
    altGPS = int(values[3]);
  }
}

void OSCgps(String theString) {
  String[] values = split(theString, ',');
  if (values.length > 4) {
    latQ = int(values[0]);
    lonQ = int(values[1]);
    v_northQ = int(values[2]);
    v_eastQ = int(values[3]);
    headingQ = int(values[4]);
  }
}

void OSCpwr(String theString) {
  String[] values = split(theString, ',');
  if (values.length > 4) {
    cmd1Q = int(values[0]);
    cmd2Q = int(values[1]);
    cmd3Q = int(values[2]);
    cmd4Q = int(values[3]);
    batteryQ = int(values[4]);
    //println(str(cmd1Q) + "," + str(cmd2Q)  + "," + str(cmd3Q) + "," + str(cmd3Q));
  }
}

void OSCimu(String theString) {
  String[] values = split(theString, ',');
  if (values.length > 6) {
    attitude.w = float(values[0]) / 16384;
    attitude.x = float(values[1]) / 16384;
    attitude.y = float(values[2]) / 16384;
    attitude.z = float(values[3]) / 16384;
    IMUheadQ = attitude.heading();
    IMUpitchQ = attitude.pitch();
    IMUrollQ = attitude.roll();
    IMUvY = int(values[4]);
    IMUvX = int(values[5]);
    IMUvZ = int(values[6]);
    //IMUaltQ = int(values[3]);
    //IMUvAltQ = int(values[4]);
  }
}

void sendTest() {
  OscMessage myMessage = new OscMessage("/t");
  osc.send(myMessage, quadAddr);
}

void sendStsReq() {
  OscMessage myMessage = new OscMessage("/s");
  osc.send(myMessage, quadAddr);
}

void sendResetStab() {
  OscMessage myMessage = new OscMessage("/r");
  osc.send(myMessage, quadAddr);
}

void sendMotorCmd(int motor, int cmd) {
  OscMessage myMessage = new OscMessage("/t");
  myMessage.add(motor);
  myMessage.add(cmd);
  osc.send(myMessage, quadAddr);
}
